/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvNavigate/publicInterfaceInc/follow_path.h
 * @Description  : 定义路径跟踪的PID控制和LQR控制结构，包括相关的函数和参数设置。 
 *                 主要包含PidFollow和LqrFollow结构体，分别用于PID控制和LQR控制的实现，以及
 *                 ModifyCtrl结构体用于路径偏差的调整。提供了一些函数用于清除控制器和执行路径跟踪。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 16:29:23
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/ 
#pragma once
#include "trajectory.h"
#include "rclcpp/rclcpp.hpp"
#include "geometry.h"
#include "pid.h"
#include "lqr_controller.h"
#include "motion_parameters.h"
struct PidFollow 
{
	PidController pid_y;
  	PidController pid_a;
	PidFollow () {
		pid_y.name_ = "Y";
    	pid_a.name_ = "A";
	}

	bool PidEnable() {
		return MotionParm::pid_params_.use;
	}

	void Clear() {
		pid_y.ClearControl();
    	pid_a.ClearControl();
	}

	float WheelBase() {
		return MotionParm::pid_params_.wheel_base;
	}

	void SetPidFilter(float current_velocity) {
		// pid参数
		const auto& params = MotionParm::pid_params_;
		bool use_low_speed = true;
		if (fabs(current_velocity) >= params.low.speed) {
			// 使用高速参数
			use_low_speed = false;
		}
		// 左右、角度选择与速度等级匹配的PID参数
		const auto& y_pid = use_low_speed ? params.low.pid_y : params.high.pid_y;
		const auto& a_pid = use_low_speed ? params.low.pid_a : params.high.pid_a;
		pid_y.SelectPid(use_low_speed, y_pid);
		pid_a.SelectPid(use_low_speed, a_pid);
	}
	
};
struct LqrFollow
{
	LQRController lqr = LQRController(50, 0.05f);
	bool param_readed = false;
	// 是否使用lqr且速度满足要求
	bool LqrEnable(double velocity) {
		const auto &p = MotionParm::lqr_params_;
		return (p.use && (abs(velocity) > p.lqr_threshold));
	}

	void read_lqr_param() {
		const auto &p = MotionParm::lqr_params_;
		lqr.eps_ = p.eps;
		lqr.iterations_ = p.iterations;
		lqr.qx_ = p.qx;
		lqr.qy_ = p.qy;
		lqr.qtheta_ = p.qtheta;
		lqr.rv_ = p.rv;
		lqr.rw_ = p.rw;
		lqr.pre_point_dist_ = p.prepoint_dist;
	}
};

// 当前轨迹纠偏量
struct ModifyCtrl {
	float angular_velocity;
	float delta_velocity_x;
	float delta_velocity_y;

	ModifyCtrl() {
		Clear();
	}

	void Clear() {
		angular_velocity = 0.0f;
		delta_velocity_x = 0.0f;
		delta_velocity_y = 0.0f;
	}

	bool CanIgnore() {
		return fabs(delta_velocity_x) < 0.00001f &&
				fabs(delta_velocity_y) < 0.00001f &&
				fabs(angular_velocity) < 0.00001f;
	}
};


extern "C"
{
	// 自动运行时，清除PID控制
	void ClearPidCtrl(float end_velocity);
	// 半自动运行时，清除PID控制
	void ClearHalfAutoPidCtrl(float end_velocity);
	// 自动运行时，纠偏逻辑
	ModifyCtrl FollowPath(const Posture* posture, Trajectory* trajectory, float linear_velocity, float a1, float a2, bool ignore_y_error);
	// 半自动运行时，纠偏逻辑
	ModifyCtrl FollowPathForHalfAuto(float x, float theta, float velocity, float a1, float a2);
}
